import codecs
import math
import config


class unit():
    def __init__(self, typename, start_s, len, startx, starty, theta, R, flag, a, b):
        # ftc: 前缓和, btc: 后缓和, circle: 圆弧, line: 直线
        self.type = typename
        self.start_s = start_s  # 该段起始里程
        self.len = len  # 该段的长度
        self.end_s = start_s + len
        self.startX = startx  # 该段的起始坐标
        self.startY = starty
        self.theta = theta
        self.R = R
        self.flag = flag
        self.befstartx = a  # 第四关键点（后缓和终点）x
        self.befstarty = b  # 第四关键点y

    def getx(self, s, r, s0):
        return s * (1 - ((1 / 40) * ((s * s * s * s) / (r * r * s0 * s0))))

    def gety(self, x, r, s0):
        x0 = getx(s0, r, s0)
        return (x * x * x) / (6 * r * x0)

    def getxy(self, the_s):
        bank = the_s - self.start_s
        if self.type == 'btc':
            bank = self.len - bank
        if self.type == 'ftc':
            xp = getx(bank, self.R, self.len)
            yp = gety(xp, self.R, self.len)
            X = (self.startX + (xp * math.cos(self.theta) + self.flag * yp * math.sin(self.theta)))
            Y = (self.startY + (xp * math.sin(self.theta) + (-self.flag) * yp * math.cos(self.theta)))
            return [X, Y]
        if self.type == 'circle':
            theta2 = (bank / self.R)
            xp = self.R * math.sin(theta2)
            yp = self.R * (1 - math.cos(theta2))
            X = (self.startX + (xp * math.cos(self.theta) + self.flag * yp * math.sin(self.theta)))
            Y = (self.startY + (xp * math.sin(self.theta) + (-self.flag) * yp * math.cos(self.theta)))
            return [X, Y]
        if self.type == 'btc':
            xp = getx(bank, self.R, self.len)
            yp = gety(xp, self.R, self.len)
            X = (self.startX - (xp * math.cos(self.theta) + self.flag * yp * math.sin(self.theta)))
            Y = (self.startY - (xp * math.sin(self.theta) + (-self.flag) * yp * math.cos(self.theta)))
            return [X, Y]
        if self.type == 'line':
            X = (self.startX + bank * math.cos(self.theta))
            Y = (self.startY + bank * math.sin(self.theta))
            return [X, Y]


def getx(s, r, s0):
    return s * (1 - ((1 / 40) * ((s * s * s * s) / (r * r * s0 * s0))))


def gety(x, r, s0):
    x0 = getx(s0, r, s0)
    return (x * x * x) / (6 * r * x0)


# 计算斜率
def getk(x1, y1, x2, y2):
    if x2 == x1:
        return 0
    return (y2 - y1) / (x2 - x1)


# 计算两点之间距离
def getdis(x1, y1, x2, y2):
    return math.sqrt(math.pow(y2 - y1, 2) + math.pow(x2 - x1, 2))


# 读取平面参数
def Readfile(address):
    f = codecs.open(address, mode='r', encoding='utf-8')
    line = f.readline()  # 以行的形式进行读取文件
    x = []
    y = []
    r = []
    len1 = []
    len2 = []
    while line:
        a = line.split()
        b = a[0:1]
        y.append(float(b[0]))
        b = a[1:2]
        x.append(float(b[0]))
        b = a[2:3]
        r.append(float(b[0]))
        b = a[3:4]
        len1.append(float(b[0]))
        b = a[4:5]
        len2.append(float(b[0]))
        line = f.readline()
    f.close()
    return x, y, r, len1, len2


#  将路线分段以列表的形式保存
def get_the_route(x=None, y=None, r=None, len1=None, len2=None):
    """
    默认从文件读取，也可传入数据
    :param x: x 数组
    :param y: y 数组
    :param r: r 数组
    :param len1: len1 数组
    :param len2: len2 数组
    :return: units 数组
    """
    units = []
    if x is None or y is None or r is None or len1 is None or len2 is None:
        x, y, r, len1, len2 = Readfile(config.line_value_file_path)
    start = [x[0], y[0]]
    sum = 0  # 记录相对里程（非实际里程）
    key_point_xy = []  # 关键点坐标
    key_point_s = []  # 关键点对应的里程
    #print(r)
    for i in range(1, 24):
        jd = [x[i], y[i]]
        end = [x[i + 1], y[i + 1]]
        R = r[i]
        arc1 = len1[i]
        arc2 = len2[i]
        k1 = getk(start[0], start[1], jd[0], jd[1])
        k2 = getk(jd[0], jd[1], end[0], end[1])
        if k2 > k1:
            flag1 = -1
        else:
            flag1 = 1
        dis1 = getdis(start[0], start[1], jd[0], jd[1])
        dis2 = getdis(jd[0], jd[1], end[0], end[1])
        dis3 = getdis(start[0], start[1], end[0], end[1])
        alafa = (math.pow(dis1, 2) + math.pow(dis2, 2) - math.pow(dis3, 2)) / (2 * dis1 * dis2)
        alafa = math.pi - math.acos(alafa)
        bta1 = len1[i] / (2 * R)
        bta2 = len2[i] / (2 * R)
        theta = math.atan(k1)
        # 前缓和曲线
        if arc1 != 0:
            x0 = getx(arc1, R, arc1)
            y0 = gety(x0, R, arc1)
            T = ((R * math.tan((alafa - bta1 - bta2) / 2)) + y0 / math.sin(bta1)) * math.sin(
                (math.pi + alafa - bta1 - bta2) / 2) / math.sin((math.pi - alafa) / 2)
            T += x0 - y0 / math.tan(bta1)
            touch1 = [jd[0] - T * math.cos(theta), jd[1] - T * math.sin(theta)]
            k3 = k1
            temp = unit('line', sum, getdis(start[0], start[1], touch1[0], touch1[1]), start[0], start[1], theta, R,
                        flag1, 1, 1)
            units.append(temp)
            sum += getdis(start[0], start[1], touch1[0], touch1[1])
            bank = 0
            key_point_xy.append([touch1[0], touch1[1]])
            key_point_s.append(sum)
            temp = unit('ftc', sum, arc1, touch1[0], touch1[1], theta, R, flag1, 1, 1)
            units.append(temp)
            sum += arc1
        #  圆弧
        if R != 0:
            theta3 = theta + (-flag1) * arc1 / (2 * R)
            T = R * math.tan((alafa - bta1 - bta2) / 2)
            if arc1 == 0:
                start_circle = [jd[0] - T * math.cos(theta3), jd[1] - T * math.sin(theta3)]
            else:
                start_circle = [touch1[0] + (x0 * math.cos(theta) + flag1 * y0 * math.sin(theta)),
                                touch1[1] + (x0 * math.sin(theta) + (-flag1) * y0 * math.cos(theta))]
            key_point_xy.append([start_circle[0], start_circle[1]])
            if arc1 == 0:
                temp = unit('line', sum, getdis(start[0], start[1], start_circle[0], start_circle[1]), start[0],
                            start[1], theta3, R, flag1, 1, 1)
                units.append(temp)
                sum += getdis(start[0], start[1], start_circle[0], start_circle[1])
            key_point_s.append(sum)
            ly = R * (alafa - bta1 - bta2)
            theta2 = (ly / R)
            xp = R * math.sin(theta2)
            yp = R * (1 - math.cos(theta2))
            key_point_xy.append([start_circle[0] + (xp * math.cos(theta3) + flag1 * yp * math.sin(theta3)),
                                 start_circle[1] + (xp * math.sin(theta3) + (-flag1) * yp * math.cos(theta3))])
            temp = unit('circle', sum, ly, start_circle[0], start_circle[1], theta3, R, flag1, 1, 1)
            units.append(temp)
            sum += ly
            key_point_s.append(sum)
        #  后缓和曲线
        if arc2 != 0:
            theta = math.atan(k2)
            x0 = getx(arc2, R, arc2)
            y0 = gety(x0, R, arc2)
            T = ((R * math.tan((alafa - bta1 - bta2) / 2)) + y0 / math.sin(bta2)) * math.sin(
                (math.pi + alafa - bta1 - bta2) / 2) / math.sin((math.pi - alafa) / 2)
            T += x0 - y0 / math.tan(bta2)
            touch2 = [jd[0] + T * math.cos(theta), jd[1] + T * math.sin(theta)]
            key_point_xy.append([touch2[0], touch2[1]])
            flag1 = -flag1
            xp = getx(arc2, R, arc2)
            yp = gety(xp, R, arc2)
            temp = unit('btc', sum, arc2, touch2[0], touch2[1], theta, R, flag1,
                        touch2[0] - (xp * math.cos(theta) + (flag1) * yp * math.sin(theta)),
                        touch2[1] - (xp * math.sin(theta) + (-flag1) * yp * math.cos(theta)))
            units.append(temp)
            sum += arc2
            key_point_s.append(sum)
            start = [touch2[0], touch2[1]]
    temp = unit('line', sum, getdis(start[0], start[1], touch1[0], touch1[1]), end[0], end[1], theta, R, flag1, 1, 1)
    units.append(temp)
    sum += getdis(start[0], start[1], start_circle[0], start_circle[1])
    return units


# 得到起始坐标的相对位置
def get_location():  # 得到起始坐标在图上的里程（非实际里程）
    units = get_the_route()
    lo_begin = units[0]
    for i in units:
        if i.type == 'btc':
            if i.befstartx < config.begin_x:
                lo_begin = i
            else:
                break
        else:
            if i.startX <= config.begin_x:
                lo_begin = i
            else:
                break
    d = getdis(lo_begin.startX, lo_begin.startY, config.begin_x, config.begin_y)
    the_s = lo_begin.start_s + d
    return the_s

#1_1对应原始线路，1_2对应新线路
units1_1 = get_the_route()
units1_2 = []


# 目标函数，输入里程，输出XY和左转右转
#flag 1:原始，2：新线路
def getxy(thes,flag):
    the_s = get_location()
    dif = thes - config.begin_s
    temp = the_s + dif
    if flag is 1:
        lo = units1_1[0]
        for i in units1_1:
            if i.start_s <= temp:
                lo = i
            else:
                break
        return [lo.getxy(temp), lo.flag]
    else:
        lo = units1_2[0]
        for i in units1_2:
            if i.start_s <= temp:
                lo = i
            else:
                break
        return [lo.getxy(temp), lo.flag]
